/*
 * main.cpp
 *
 *  Created on: 26.01.2014
 *      Author: Benni
 *
 *      Main source file for eigensolver program.
 */

#include <iostream>
#include <math.h>
#include <fstream>
#include <cmath>
#include <vector>
#include <cstdlib>
#include <ctime>
using namespace std;

int main() {

//Define arrays for test stiffness matrix in csr format (Originally 6x6): K

	int iDimCSR = 16;

	int aRowStiff[16] = { 0, 0, 1, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 4, 5, 5 };
	int aColStiff[16] = { 0, 1, 0, 1, 2, 1, 2, 3, 2, 3, 4, 3, 4, 5, 4, 5 };
	double aCoefStiff[16] = { 1.035, 2.345, 2.345, 5.765, 0.596, 0.596, 4.232,
			0.975, 0.975, 5.678, 0.231, 0.231, 5.654, 0.564, 0.564, 7.984 };

//Define arrays for test mass matrix in csr format (Originally 6x6): M

	int aRowMass[16] = { 0, 0, 1, 1, 1, 2, 2, 2, 3, 3, 3, 4, 4, 4, 5, 5 };
	int aColMass[16] = { 0, 1, 0, 1, 2, 1, 2, 3, 2, 3, 4, 3, 4, 5, 4, 5 };
	double aCoefMass[16] = { 15.035, 22.345, 22.345, 35.765, 2.596, 2.596,
			14.232, 2.975, 2.975, 15.658, 3.231, 3.231, 35.654, 8.564, 8.564,
			42.984 };
//**************************************************************************************************************************
//**************************DECLARATION OF ALL VARIABLES********************************************************************
//**************************************************************************************************************************

// 1. Variables used within Subspace Iteration

	int iSubspaceDim = 3;											//Dimension of subspace
	int iInitialDim = 6;											//Dimension of initial system
	double aSubspace[iInitialDim][iSubspaceDim];					//Matrix with starting vectors spanning the subspace
	double aSubK[iSubspaceDim][iSubspaceDim];						//Subspace K-Matrix
	double aHelpK[iSubspaceDim][iInitialDim];						//Storage for product x1^T*K
	double aSubM[iSubspaceDim][iSubspaceDim]; 						//Subspace M-Matrix
	double aHelpM[iSubspaceDim][iInitialDim]; 						//Storage for product x1^T*M
	double aSubTEMP[iInitialDim][iSubspaceDim];						//Temporary storage of matrix with subspace vectors
	double dEpsilon=pow(10,-24);									//Tolerance value for convergence measure
	double dQScal=0;												//Storage of scalar products of subspace eigenvectors
	double aTol[iSubspaceDim];										//Vector storing tolerance values
	int iCOUNT=0;													//Counter for Subspace iterations
	int iTC=0;														//True counter for subspace iterations

// 2. Variables used within Jacobi-Iteration

	int iSweep=0;													//Counter for sweep number
	int iOffdiagDim=(iSubspaceDim*iSubspaceDim-iSubspaceDim)/2;		//Dimension for upper off-diagonal element storage
	double aPMatrix[iSubspaceDim][iSubspaceDim];					//Rotation Matrix for Jacobi Rotations
	double dCoupK[iOffdiagDim];										//Stiffness Coupling factor vector
	double dCoupM[iOffdiagDim];										//Mass Coupling factor vector
	int iCoupCount=0;												//Counter for Coupling Vectors
	double dK_ii=0;													//Working factor K'ii
	double dK_jj=0;													//Working factor K'jj
	double dK_zz=0;													//Working factor K'
	double dSQRT=0;													//Storage of intermediate result (root-term)
	double dx=0;													//Denominator x for alpha and gamma computation
	double dAlpha=0;												//Alpha-factor for rotation matrix
	double dGamma=0;												//Gamma-factor for rotation matrix
	double aHelpPK[iSubspaceDim][iSubspaceDim];						//Storage for P1^T*K
	double aHelpPM[iSubspaceDim][iSubspaceDim];						//Storage for P1^T*M
	double aSumP[iSubspaceDim][iSubspaceDim];						//Storage of rotation Matrix Product P1*P2*...*Pn
	double aPMatrixOld[iSubspaceDim][iSubspaceDim];					//Storage of P-Matrix from previous iteration
	double aSubKOld[iSubspaceDim][iSubspaceDim];					//Storage of rotated Stiffness Matrix from previous sweep
	double aSubMOld[iSubspaceDim][iSubspaceDim];					//Storage of rotated Mass Matrix from previous sweep
	double aCONV [iSubspaceDim+2*iOffdiagDim];						//Convergence matrix, stores convergence criteria for lamda, k, m
	int iS=-12;														//Convergence exponent: aCONV<=10^(iS)
	double dLambda=0;												//Eigenvalue of current iteration
	double dLambdaOld=0;											//Eigenvalue of previous iteration
	double dLambdaConv=0;											//Eigenvalue convergence ratio
	double aSubPhi[iSubspaceDim][iSubspaceDim];						//Eigenvector matrix of Subspace
	double aSubDiag[iSubspaceDim][iSubspaceDim];					//Diagonal matrix with 1/sqrt(m_ii)
	int iJ=0;														//Jacobi iteration counter for convergence criteria
	int iK=0;														//Counter for Stiffness Coupling factor
	int iM=0;														//Counter for Mass Coupling factor

//**************************************************************************************************************************
//************************************END OF VARIABLE DECLARATION***********************************************************
//**************************************************************************************************************************

// Create initial matrix of subspace vectors X_1 from random numbers

	srand(time(0));

	for (int i = 0; i < iInitialDim; i++) {
		for (int j = 0; j < iSubspaceDim; j++) {
			double ia = 1-abs(i-j) ;
			double ib = 5+(abs(j-i)) ;
			aSubspace[i][j] = ia / ib;
		}
	}

//Initialize aTol with 1 entries
	for(int n=0;n<iSubspaceDim;n++){
		aTol[n]=1;
	}

// Calculate diagonal quotients k_ii/m_ii for Unit vector choice when using improved method for starting iteration vectors
//
//	double aDiagQuot[iInitialDim][2];
//	int k=0;
//
//	for (int i = 0; i < iDimCSR; i++) {
//		if (aRowStiff[i] == aColStiff[i]) {
//			for (int j = 0; j < iDimCSR; j++) {
//				if (aRowMass[j] == aColMass[j]){
//					if(aRowStiff[i]==aRowMass[j]){
//						aDiagQuot[k][0]=aCoefStiff[i]/aCoefMass[j];
//						aDiagQuot[k][1]=aRowMass[i];
//						k++;
//						break;
//					}
//				}
//			}
//		}
//	}
//
//Check values via IOstrean
//	for (int i = 0; i < iInitialDim; i++) {
//		cout << aDiagQuot[i][0] << " ------ " << aDiagQuot[i][1] << endl;
//	}
iCOUNT=0;
while(iCOUNT<iSubspaceDim){
	if(aTol[iCOUNT]>=dEpsilon){
		iTC++;

//**************************************************************************************************************************
//Transform Stiffness Matrix into Subspace via: K'= X_1^T*K*X_1			X_1=aSubspace[i][j] and X_1^T=aSubspace[j][i]

//Multiplication X_1^T*K=aHelpK
	for (int i = 0; i < iSubspaceDim; i++) {
		for (int j = 0; j < iInitialDim; j++) {
			aHelpK[i][j] = 0;
			for (int k = 0; k < iInitialDim; k++) {
				for (int n = 0; n < iDimCSR; n++) {
					if (aColStiff[n] == j) {
						if (aRowStiff[n] == k) {
							aHelpK[i][j] += aCoefStiff[n] * aSubspace[k][i];
						}
					}
				}
			}
		}
	}
//Multiplication aHelpK*X_1=aSubK

	for (int i = 0; i < iSubspaceDim; i++) {
		for (int j = 0; j < iSubspaceDim; j++) {
			aSubK[i][j] = 0;
			for (int k = 0; k < iInitialDim; k++) {
				aSubK[i][j] += aHelpK[i][k] * aSubspace[k][j];
			}
		}
	}

//	Check output
//	for (int i=0;i<iSubspaceDim;i++){
//		for (int j=0;j<iSubspaceDim;j++){
//			cout << aSubK[i][j] << endl;
//		}
//	}

//**************************************************************************************************************************
//Transform Mass Matrix into Subspace via: M'= X_1^T*M*X_1			X_1=aSubspace[i][j] and X_1^T=aSubspace[j][i]

//Multiplication X_1^T*M=aHelpM
	for (int i = 0; i < iSubspaceDim; i++) {
		for (int j = 0; j < iInitialDim; j++) {
			aHelpM[i][j] = 0;
			for (int k = 0; k < iInitialDim; k++) {
				for (int n = 0; n < iDimCSR; n++) {
					if (aColMass[n] == j) {
						if (aRowMass[n] == k) {
							aHelpM[i][j] += aCoefMass[n] * aSubspace[k][i];
						}
					}
				}
			}
		}
	}

//Multiplication aHelpM*X_1=aSubM
	for (int i = 0; i < iSubspaceDim; i++) {
		for (int j = 0; j < iSubspaceDim; j++) {
			aSubM[i][j] = 0;
			for (int k = 0; k < iInitialDim; k++) {
				aSubM[i][j] += aHelpM[i][k] * aSubspace[k][j];
			}
		}
	}

//Checking output:
//	for (int i = 0; i < iSubspaceDim; i++) {
//		for (int j = 0; j < iSubspaceDim; j++) {
//			cout << aSubM[i][j] << endl;
//		}
//	}
//
//	for (int i = 0; i < iSubspaceDim; i++) {
//		for (int j = 0; j < iSubspaceDim; j++) {
//			cout << aSubK[i][j] << endl;
//		}
//	}

//**************************************************************************************************************************
//**************************************************************************************************************************
//Solve transformed system with full matrices K' and M' by generalized Jacobi-Algorithm

//Initialize convergence matrix to 1 entries
	for (int n=0; n<iSubspaceDim+2*iOffdiagDim;n++){
			aCONV[n]=1;
	}

//Initialize aPMatrixOld as Unit matrix for first Sweep!
	for (int n=0;n<iSubspaceDim;n++){
		for (int m=0;m<iSubspaceDim;m++){
			if (n==m){
				aPMatrixOld[n][m]=1;
			}
			else{
				aPMatrixOld[n][m]=0;
			}
		}
	}

//START JACOBI ITERATION
	iJ=0;
	iSweep=0;
	while (iJ<iSubspaceDim+2*iOffdiagDim){
	if(aCONV[iJ]>=pow(10,iS)){

	iSweep++;

//Store rotated stiffness matrix in aSubKOld:
	for (int n=0; n<iSubspaceDim;n++){
		for (int m=0; m<iSubspaceDim;m++){
			aSubKOld[n][m]=aSubK[n][m];
		}
	}
//Store rotated mass matrix in aSubMOld:
	for (int n=0; n<iSubspaceDim;n++){
		for (int m=0; m<iSubspaceDim;m++){
			aSubMOld[n][m]=aSubM[n][m];
		}
	}


//Initialize sweep loop over upper-diagonal elements K_i,j and M_i,j for j>=i
for (int i=0;i<iSubspaceDim;i++){
	for (int j=i+1;j<iSubspaceDim;j++){

//Initialize P-Matrix as a unit matrix
	for (int n = 0; n < iSubspaceDim; n++) {
		for (int m = 0; m < iSubspaceDim; m++) {
			if (n==m){
				aPMatrix[n][m] = 1;
			}
			else{
				aPMatrix[n][m] = 0;
			}
		}
	}

//Calculate working factors for alpha and gamma:
	dK_ii=aSubK[i][i]*aSubM[i][j]-aSubM[i][i]*aSubK[i][j];
	dK_jj=aSubK[j][j]*aSubM[i][j]-aSubM[j][j]*aSubK[i][j];
	dK_zz=aSubK[i][i]*aSubM[j][j]-aSubK[j][j]*aSubM[i][i];

	dSQRT=sqrt(pow(dK_zz/2,2)+dK_ii*dK_jj);
	dx=dK_zz/2+copysign(dSQRT,dK_zz);

//Calculate alpha and gamma:
	dAlpha=dK_jj/dx;
	dGamma=-dK_ii/dx;

//Fill P-Matrix
	aPMatrix[i][j]=dAlpha;
	aPMatrix[j][i]=dGamma;

//**************************************************************************************************************************
//Calculate P^T*K*P, for looping purpose store final result again in aSubK

//Multiplication P^T*K=aHelpPK:
	for (int n = 0; n < iSubspaceDim; n++) {
		for (int m = 0; m < iSubspaceDim; m++) {
			aHelpPK[n][m]=0;
			for (int k = 0; k < iSubspaceDim; k++) {
				aHelpPK[n][m] += aPMatrix[k][n] * aSubK[k][m];
			}
		}
	}

//Multiplication aHelpPK*P=aSubK (new):
	for (int n = 0; n < iSubspaceDim; n++) {
		for (int m = 0; m < iSubspaceDim; m++) {
				aSubK[n][m]=0;
			for (int k = 0; k < iSubspaceDim; k++) {
				aSubK[n][m] += aHelpPK[n][k] * aPMatrix[k][m];
			}
		}
	}

//**************************************************************************************************************************
//Calculate P^T*M*P, for looping purpose store final result again in aSubM

//Multiplication P^T*M=aHelpPM:
	for (int n = 0; n < iSubspaceDim; n++) {
		for (int m = 0; m < iSubspaceDim; m++) {
			aHelpPM[n][m]=0;
			for (int k = 0; k < iSubspaceDim; k++) {
				aHelpPM[n][m] += aPMatrix[k][n] * aSubM[k][m];
			}
		}
	}

//Multiplication aHelpPM*P=aSubM (new):
	for (int n = 0; n < iSubspaceDim; n++) {
		for (int m = 0; m < iSubspaceDim; m++) {
				aSubM[n][m]=0;
			for (int k = 0; k < iSubspaceDim; k++) {
				aSubM[n][m] += aHelpPM[n][k] * aPMatrix[k][m];
			}
		}
	}

//**************************************************************************************************************************
//Multiply current aPMatrixOld with actual working P-Matrix and store in aSumP
	for (int n = 0; n < iSubspaceDim; n++) {
		for (int m = 0; m < iSubspaceDim; m++) {
				aSumP[n][m]=0;
			for (int k = 0; k < iSubspaceDim; k++) {
				aSumP[n][m] += aPMatrixOld[n][k] * aPMatrix[k][m];
			}
		}
	}

//Overwrite aPMatrixOld with current P-Matrix Product:
	for (int n=0; n<iSubspaceDim;n++){
		for (int m=0; m<iSubspaceDim;m++){
			aPMatrixOld[n][m]=aSumP[n][m];
		}
	}
//##############
//#END OF SWEEP#
//##############
	}
}

//Calculate coupling factors (convergence) from actual k and m coefficients (!!)
	iCoupCount=0;
	for (int n=0;n<iSubspaceDim;n++){
		for (int m=n+1;m<iSubspaceDim;m++){
			dCoupK[iCoupCount]=sqrt((aSubK[n][m]*aSubK[n][m])/(aSubK[n][n]*aSubK[m][m]));
			dCoupM[iCoupCount]=sqrt((aSubM[n][m]*aSubM[n][m])/(aSubM[n][n]*aSubM[m][m]));
			iCoupCount++;
		}
	}

//Calculate convergence values for Lambda, K and M:
	for (int n=0;n<iSubspaceDim;n++){
			dLambda=aSubK[n][n]/aSubM[n][n];
			dLambdaOld=aSubKOld[n][n]/aSubMOld[n][n];
			dLambdaConv=(abs(dLambda-dLambdaOld)/dLambda);
//Fill convergence vector
			aCONV[n]=dLambdaConv;
	}
	iK=0;
	for	(int n=iSubspaceDim;n<iSubspaceDim+iOffdiagDim;n++){
			aCONV[n]=dCoupK[iK];
			iK++;
	}
	iM=0;
	for	(int n=iSubspaceDim+iOffdiagDim;n<iSubspaceDim+2*iOffdiagDim;n++){
	aCONV[n]=dCoupM[iM];
	iM++;
	}

//#########################
//#END OF JACOBI ITERATION#
//#########################
	iJ=0;
		}
		else{
			iJ++;

		}
	}

	cout << "Jacobi iteration converged after " << iSweep <<" Sweeps!" << endl;

// Calculate mass diagonal matrix aSubDiag
	for (int n=0;n<iSubspaceDim;n++){
		for (int m=0;m<iSubspaceDim;m++){
			if (m==n){
				aSubDiag[n][m]=1/sqrt(aSubM[n][m]);
			}
			else{
				aSubDiag[n][m]=0;
			}
		}

	}

//Calculate eigenvector matrix aSubPhi
	for (int n = 0; n < iSubspaceDim; n++) {
		for (int m = 0; m < iSubspaceDim; m++) {
				aSubPhi[n][m]=0;
			for (int k = 0; k < iSubspaceDim; k++) {
				aSubPhi[n][m] += aSumP[n][k] * aSubDiag[k][m];
			}
		}
	}

//LEAVING SUBSPACE
//**************************************************************************************************************************
//Updating Subspace vectors with calculated eigenvetors of reduced system
	for(int n=0;n<iInitialDim;n++){
		for(int m=0;m<iSubspaceDim;m++){
			aSubTEMP[n][m]=aSubspace[n][m];
		}
	}

	for(int n=0;n<iInitialDim;n++){
		for(int m=0;m<iSubspaceDim;m++){
			aSubspace[n][m]=0;
			for (int k=0;k<iSubspaceDim;k++){
				aSubspace[n][m]+=aSubTEMP[n][k]*aSubPhi[k][m];
			}
		}
	}

//Calculate tolerance criterion
	for(int n=0;n<iSubspaceDim;n++){
		dQScal=0;
		for (int m=0;m<iSubspaceDim;m++){
				dQScal+=aSubPhi[m][n]*aSubPhi[m][n];

		}
		aTol[n]=sqrt(1-aSubKOld[n][n]*aSubKOld[n][n]/(dQScal));
	}

//###########################
//#END OF SUBSPACE ITERATION#
//###########################
	iCOUNT=0;
	}
	else{
		iCOUNT++;
	}
}
cout << "Subspace iteration converged after " << iTC <<" Iterations!" << endl;



//Checking output:
//	for (int i = 0; i < iSubspaceDim; i++) {
//		for (int j = 0; j < iSubspaceDim; j++) {
//			cout << aSubM[i][j] << endl;
//		}
//	}

	for (int i = 0; i < iSubspaceDim; i++) {
//		for (int j = 0; j < iSubspaceDim; j++) {
			cout << aSubK[i][i] << endl;
//		}
	}

	for (int i = 0; i < iInitialDim; i++) {
		for (int j = 0; j < iSubspaceDim; j++) {
			cout << aSubspace[i][j] << endl;
		}
	}

return 0;
}

//Check ouput******************************
//for (int i = 0; i < iSubspaceDim; i++) {
//	for (int j = 0; j < iInitialDim; j++) {
//		cout << aHelpK[i][j] << endl;
//	}
//}
